#include "TDBase.h"

PathFinder::PathFinder()
{
}

bool PathFinder::Find (const StageMap& map, int beginIdx, int endIdx, MapPath& path)
{
	if ((int)_mapNodes.size() != map.Size())
	{
		_mapNodes.resize(map.Size());
	}

	OpenList					openList(_mapNodes);
	dynamic_bitset				closedList(_mapNodes.size());
	const MapBrick&				goalBrick = map[endIdx];

	//add the starting node to the open list
	_mapNodes[beginIdx].Parent = -1;
	_mapNodes[beginIdx].Cost = 0;
	_mapNodes[beginIdx].Total = map[beginIdx].DistanceTo(goalBrick);
	openList.Push (beginIdx);

	while(!openList.IsEmpty()) 
	{
		//	current node=node from open list with the lowest cost
		int bestIdx =openList.Pop();

		if (bestIdx == endIdx)
		{
			FillPath (map, beginIdx, endIdx, path);
			return true;
		}
		else
		{
			closedList[bestIdx] = true;
			const MapBrick& bestBrick = map[bestIdx];

			for (int i=0; i<4; i++)
			{
				int newIdx = bestBrick.GetNeighborIdx(i);

				if (newIdx>=0 && (!openList.Contain(newIdx)) && (!closedList[newIdx]))
				{
					const MapBrick& newBrick = map[newIdx];

					if (newBrick.IsWalkable())
					{
						Node& newNode = _mapNodes[newIdx];

						newNode.Parent = bestIdx;
						newNode.Cost = _mapNodes[bestIdx].Cost +1;
						newNode.Total = newNode.Cost +newBrick.DistanceTo(goalBrick);

						openList.Push (newIdx);
					}
				}
			}
		}
	}

	return false;
}

void PathFinder::FillPath (const StageMap& map, int beginIdx, int endIdx, MapPath& path)
{
	int idx = endIdx;
	path.PushFront(idx);

	while (idx !=beginIdx)
	{
		idx = _mapNodes[idx].Parent;
		path.PushFront(idx);
	}

	path.ConstructSegments(map);
}

void PathFinder::Explore(const StageMap& map, int start, int block, dynamic_bitset& brickBits) const
{
	assert (map.Size() == (int)brickBits.size());

	std::deque<int>  seedQue;
	seedQue.push_front(start);

	while (!seedQue.empty())
	{
		int brickIdx = seedQue.back();
		seedQue.pop_back();
		brickBits[brickIdx] = true;

		for (int i=0; i<4; i++)
		{
			int newIdx = map[brickIdx].GetNeighborIdx(i);
			
			if (newIdx>=0 && newIdx!=block && (!brickBits[newIdx]) && map[newIdx].IsWalkable())
			{
				seedQue.push_front(newIdx);
			}
		}
	}
}